#!/usr/bin/env python
# coding:utf-8
# created by Peixuan Shu
# 2023.4.7
"""
tello ground control:
1. connect and reconnect (launch tello driver ROS node)
2. display connection, battery and pose_source state
3. forward pose data to a uniform topid /telloi/pose
"""
"""
TODO:
1. pos_source_ok color display
2. tello_config update
3. video transfer
4. more command number pub
"""

import tkinter as tk
import tkinter.messagebox 
# import ctypes
import subprocess
import time
import os
# from datetime import datetime

from .topframe import TopFrame
from .consoleframe import ConsoleFrame
from .uavnode import UAVnode

import rospy
from std_msgs.msg import Int32


def main():
    print("tello ground control GUI started!")
    root = tk.Tk()

    root.resizable(1,1) # window size changable

    normal_width = 1210 # normal width
    normal_height = 880 # normal height
    height_times = 0.5 # real_height / screen_height 

    root.scale_factor = int(root.winfo_screenheight() * height_times) / normal_width
    win_width = int(normal_height * root.scale_factor)
    win_height = int(normal_width * root.scale_factor)
    off_y = int( (root.winfo_screenheight() - 600) / 4.0 )
    off_x = int( (root.winfo_screenwidth() - 800) / 2.0 )
    root.geometry("{}x{}+{}+{}".format(win_height, win_width, off_x, off_y))
    # root.geometry("{}x{}+{}+{}".format(win_width, win_height, off_x, off_y))

    root.title('Tello Swarm Ground Control')

    init_time = time.time()

    ################### create the log file folder #################
    # record data file path "../../data_log/log%month%date%hour%min%sec"
#     folder = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
#     log_path = folder + '/data_log/log' + datetime.now().strftime("%Y%m%d") + \
# '-' + datetime.now().strftime("%H%M%S")
#     os.mkdir(log_path) # do not use os.makedirs() for error reporting

    gui = Gui(root, init_time)

    root.protocol('WM_DELETE_WINDOW', gui.__del__)

    # Load the image file from disk.
    folder = os.path.dirname(os.path.abspath(__file__)) # absolute path of this folder 
    icon = tk.PhotoImage(file=os.path.dirname(os.path.dirname(folder))+"/pictures/DJI-Tello.png")
    # Set it as the window icon.
    root.iconphoto(True, icon)

    root.mainloop()

class Gui:
    """ 
    Main GUI object 
    It consists of a Top frame, which is the menu and a main frame,
    which contains all the visual logs to the Crazyflie.
    """
    def __init__(self, root, init_time, log_path="."):
        self.root = root
        self.init_time = init_time
        self.log_path = log_path

        ############ Read tello config ###########
        folder = os.path.dirname(os.path.abspath(__file__)) # absolute path of this folder 
        self.config_folder = os.path.dirname(os.path.dirname(folder)) # up up folder
        self.tello_config = self.config_folder + "/config/tello_config.yaml"

        self.tellodriver_nodes = {}
        self.uav_nodes = {}

        self.consoleframe = ConsoleFrame(root, self)
        self.topframe = TopFrame(root, self)
        
        self.topframe.pack(side='top')
        self.consoleframe.pack(side='bottom')

        self.scale = root.scale_factor

        self.print_console("tello ground control GUI started!")

        # launch roscore
        self.roscore_bygui = False
        self.rosmaster_pid = subprocess.getoutput("pgrep rosmaster")
        if not self.rosmaster_pid:
            self.roscore = subprocess.Popen(['/bin/bash', '-i', '-c', 'roscore'],start_new_session=True)
            self.roscore_bygui = True
            self.print_console("rosmaster started!")
        else:
            self.roscore_bygui = False
            self.print_console("rosmaster already exists! pid: {}".format(self.rosmaster_pid))

        # # launch switch node for uwb pos read and simultaneous takeoff and land
        # self.switch_node_bygui = False # if switch node is launched by this gui
        # self.switch_node_pid = subprocess.getoutput("pgrep switch_node")
        # if not self.switch_node_pid:
        #     cmd = 'roslaunch switch switch.launch'
        #     self.switch_node = subprocess.Popen(['/bin/bash', '-i', '-c', cmd],start_new_session=True)
        #     self.switch_node_bygui = True
        #     self.print_console("switch_node started!")
        # else:
        #     self.switch_node_bygui = False
        #     self.print_console("switch_node already exists! pid: {}".format(self.switch_node_pid))      

        ############ ROS initialize ############
        time.sleep(0.1)
        try:
            rospy.init_node('tellogc_node', anonymous=True)
        except:
            pass # avoid overinit
        self.cmd_pub = rospy.Publisher('/command', Int32, queue_size=10)  
        self.land_num_pub = rospy.Publisher('/land_num', Int32, queue_size=10)
        self.takeoff_num_pub = rospy.Publisher('/takeoff_num', Int32, queue_size=10)  

    def __del__(self):
        """ close window """
        try:
            self.root.destroy() # if window not closed, close it
        except:
            pass
        """ delete uav_nodes and finish threads """ 
        for id in self.uav_nodes:
            self.uav_nodes[id].__del__()
        """if switch_node/roscore is launched by this GUI, kill it when exiting"""
        # if self.switch_node_bygui:
        #     self.switch_node.terminate()
        #     self.switch_node.wait()
        if self.roscore_bygui:
            self.roscore.terminate()
            self.roscore.wait()
        """ kill tello driver ROS nodes"""
        for id in self.tellodriver_nodes:
            self.tellodriver_nodes[id].terminate()
        for id in self.tellodriver_nodes:
            self.tellodriver_nodes[id].wait()         

    def update_tello_config(self, tello_config_path):
        """update self.tello_config """
        if tello_config_path.endswith(".yaml"):
            self.tello_config = tello_config_path
            self.print_console('Load tello_config: {}'.format(self.tello_config))
            self.print_console('Please reconnect all nodes to refresh them')
            self.showinfo_msg('Successfully load: {}.\n \nPlease reconnect all nodes to refresh them'.format(self.tello_config))  
            self.topframe.config_entry.delete(0,"end")
            self.topframe.config_entry.insert(0, self.tello_config)
        else:
            self.print_console('[Error] The config file is invalid: {}'.format(tello_config_path))  
            self.warning_msg('[Error] The config file is invalid: {}'.format(tello_config_path))  

    def connect(self, id):
        """ add uavnode and connect to tello """

        ### add uav_node ###
        if id in self.uav_nodes:
            self.uav_nodes[id].__del__() # close the former uav node
        self.uav_nodes[id] = UAVnode(id, self.tello_config, self)

        ### check if id is in tello_config yaml
        if self.uav_nodes[id].ip == "not found":
            self.uav_nodes[id].__del__()
            self.print_console("[Error] id {} not found in config file: {}".format(id, self.tello_config))
            defaultbg = self.root.cget('bg') # default button color
            self.topframe.uav_button[id].configure(bg=defaultbg) 
        else:
            ### launch tello driver ###
            if id in self.tellodriver_nodes:
                self.tellodriver_nodes[id].terminate() # close the former tello driver
                # self.tellodriver_nodes[id].wait()
                # self.warning_msg('UAV exists!')
                self.print_console('reconnecting to tello'+str(id)+" ip: " + self.uav_nodes[id].ip + " pos_src: " + self.uav_nodes[id].pose_source)
            else:
                self.print_console('connecting to tello'+str(id)+" ip: " + self.uav_nodes[id].ip + " pos_src: " + self.uav_nodes[id].pose_source)
            if id < 10:
                cmd = "roslaunch tello_driver tello_driver_single_1-9.launch tello_id:=" + str(id) + " tello_ip:=" + self.uav_nodes[id].ip
            else:
                cmd = "roslaunch tello_driver tello_driver_single_10_plus.launch tello_id:=" + str(id) + " tello_ip:=" + self.uav_nodes[id].ip
            self.tellodriver_nodes[id] = subprocess.Popen(['/bin/bash', '-i', '-c', cmd],shell=False,start_new_session=True)

    def warning_msg(self, msg):
        tkinter.messagebox.showwarning('warning', msg)
    
    def showinfo_msg(self, msg):
        tkinter.messagebox.showinfo('', msg)

    def print_console(self, msg, color = "green"):
        self.consoleframe.print_console(msg, color)

if __name__ == '__main__':
    main()
